/**
    Copyright 2015 Table Rider
    By: Anders Nylander
*/

#include <cstdarg>
#include <time.h>
#include <pthread.h>
#include "ComServer.h"
#include "include/ComClient.h"
#include "CordSystem.h"
#include "SerialCom.h"
#include "Dope.h"
#include "include/tblrdr_cmds.h"
int John;
CordSystem *cord;
Dope *dope;
SerialCom *serial;

void* doeT_Handler(void* obj);
const char* parseCmd(void* sending);

int main(int argc, char* argv[])
{
    pthread_t doeThread;
    John = 0;
    const char* p;
    int port;
    ComServer server;
    static SerialCom s;
    serial = &s;
    static CordSystem cs(0.0, 0.0, serial);
    cord = &cs;
    static Dope doep(&cs);
    dope = &doep;
    if (argc < 2) port = 4444;
    else port = atoi(argv[1]);
    printf("%s\n", p = server.init('\0',port));
    if (p[0] == 'B') return -1;
    if (serial->getfd() == -1)
    {
        printf("Serial communication init failed.\n");
        return -1;
    }

    const char* msg = NULL;
    const char* sending = NULL;
    //const char* pq=NULL;

    pthread_create(&doeThread, NULL, doeT_Handler, (void*)NULL);

    while(1)
    {
        server.listen_connection();
        sending = server.receive_msg();
        if(sending != NULL)
        {
            if(sending[0] != '\0')
            {
                msg = parseCmd((void*)sending);
                sending=NULL;
                if (msg[0]!='\0')printf("%s\n", msg);
                server.send_msg((void*)msg, strlen(msg));
            }
        }
    }
    return 0;
}

const char* parseCmd(void* sending)
{
    char* send = (char*)sending;
    short cmd = (short)send[0];
    static char buf[2000];
    for(int i=0; i<2000; i++)buf[i] = '\0';
    switch(cmd)
    {
    case 0:
        break;
    case TBLRDR_GETPOS:
    {
        double x= cord->getPosX();
        double y= cord->getPosY();
        sprintf(buf, "x:%f\ny:%f\n",x,y);
        return buf;
        break;
    }
    case TBLRDR_SETPOS:
    {
        double x = (double)*(double*)(send+2);
        double y = (double)*(double*)(send+10);
        cord->setPosX(x);
        cord->setPosY(y);
        sprintf(buf, "x:%f\ty:%f\n", x,y);
        return buf;
        break;
    }
    case TBLRDR_GETANGLE:
    {
        sprintf(buf, "Angle: %f\n", cord->getDirection());
        return buf;
        break;
    }
    case TBLRDR_SETANGLE:
    {
        double arg = (double)*(double*)(send+2);
        cord->setDirection(arg);
        sprintf(buf, "Setting angle to: %f\n", arg);
        return buf;
        break;
    }
    case TBLRDR_MOVETO:
    {
        double x = (double)*(double*)(send+2);
        double y = (double)*(double*)(send+10);
        cord->moveTo(x, y);
        sprintf(buf, "Moving to coordinates %f,%f\n", x,y);
        return buf;
        break;
    }
    case TBLRDR_MOVEBY:
    {
        double x = (double)*(double*)(send+2);
        double y = (double)*(double*)(send+10);
        cord->moveBy(x, y);
        sprintf(buf, "Moving by coordinates %f,%f\n", x,y);
        return buf;
        break;
    }
    case TBLRDR_JOHN:
    {
        unsigned long ip = (unsigned long)*(unsigned long*)(send+2);
        unsigned short port = (unsigned short)*(unsigned short*)(send+6);
        uint8_t *bip = (uint8_t*)&ip;
        sprintf(buf, "\"John\" is now %d.%d.%d.%d:%d! I will follow\n", bip[0], bip[1], bip[2], bip[3], port);
        dope->setDoe(ip, port);
        return buf;
        break;
    }
    case TBLRDR_MOVEFWD:
    {
        double amp = (double)*(double*)(send+2);
        cord->moveStraight(amp);
        sprintf(buf, "Moving straight by %f pulses\n", amp);
        return buf;
        break;
    }
    case TBLRDR_TURNTO:
    {
        double amp = (double)*(double*)(send+2);
        cord->turnTo(amp);
        sprintf(buf, "Turning to %f degrees\n", amp);
        return buf;
        break;
    }
    case TBLRDR_TURNBY:
    {
        double amp = (double)*(double*)(send+2);
        cord->turnBy(amp);
        sprintf(buf, "Turning by %f degrees\n", amp);
        return buf;
        break;
    }/*
    case TBLRDR_REQPOS:
    {
        printf("ASDF!\n");
        unsigned short* a = (unsigned short*) malloc(2*sizeof(char));
        a[0] = TBLRDR_GETPOS;
        ComClient client;
        const char* c;
        unsigned long* ip = (unsigned long*)(send+2);
        unsigned short* port = (unsigned short*)(send+6);
        printf("Finding John...\n");
        printf("%s\n", client.init(*ip, *port));
        printf("%d\n", client.send_msg((void*)a, 2));
        printf("%s\n", c = client.receive_msg());
        client.close_connection();
        return c;
        break;
    }*/
    case TBLRDR_NEXTSTEP:
    {
        cord->doNextStep();
        sprintf(buf, "We have %d steps left\n", cord->getStepAmount());
        return buf;
    }
    case TBLRDR_ALLSTEPS:
    {
        sprintf(buf, "Attempting to perform all %d remaining steps...\n", cord->getStepAmount());
        while(cord->getStepAmount() > 0){
            cord->doNextStep();
        }
        return buf;
        break;
    }
    default:
        return serial->send_msg((void*)sending);
    }
    return serial->send_msg((void*)sending);
}

/** doeT_Handler is the thread that handles Doe's autonomous movements.
	Takes no arguments, returns no arguments.
*/
void* doeT_Handler(void* obj){
	time_t myTime;
	time(&myTime);
	if (obj != NULL) printf("Non-NULL ptr passed to child thread\n");
	while(1){
		if (time(NULL)-myTime > 0 && dope->isDoe()){
			dope->update();
			time(&myTime);
            usleep(900000); /// sleep for 900ms
		}
		usleep(100);
	}
	return NULL;
}
